#include "PhysicsEngine.h"

PhysicsEngine::PhysicsEngine():rend(RenderManager::getInstance())
{
}

void PhysicsEngine::calcFriction(RigidBody &b){
    if(Collision::hasCollidedDownAtOffset(b,vector3<float>(0,-0.1,0))
       && b.s.velocity.magsqr()>FRICTION_THRES)
    {
        //easing method -x^2+1 -> based on concept that friction increases as velocity decreases
        float stepsqr=GS->getStepSize()*GS->getStepSize(),
              xsqr=b.s.velocity.magsqr()/stepsqr,
              ease = -xsqr+1;
        b.s.velocity += (-b.s.velocity * GS->getFriction() * FRICTION_CONSTANT * TIMESTEP * ease);
    }
}

void PhysicsEngine::update(){
    //to-do: continuous collision detection
    for(int i=0;i<bodies.size();i++){

     if(bodies[i]->isDynamic){
         bodies[i]->update();
         if(bodies[i]->isCollidable){

             calcFriction(*bodies[i]);
             resolveCollisionWithBodies(*bodies[i]);
             resolveCollisionWithSurface(*bodies[i]);

         }

     }

        //resolveContacts();
    }
}

void PhysicsEngine::resolveCollisionWithBodies(RigidBody &b){
    //slow algo. replace with O(n) algo
        for(int i=0;i<bodies.size();i++){
            if(bodies[i]==&b || !bodies[i]->isCollidable)continue;

            if(b.geometry.hasContactedAABB(bodies[i]->geometry)){
                float interval = TIMESTEP/2.;
                bool contacted=true;
                //continuous collision detecton: bisection method
                do{

                    if(contacted)
                    b.translate(-b.d.velocity * interval);
                    else interval/=2,b.translate(b.d.velocity * interval);

                    contacted=b.geometry.hasContactedAABB(bodies[i]->geometry);
                }while(interval>BISECTION_LIMIT && contacted);

                b.s.velocity = refVelocity(b.d.velocity,-b.d.velocity.unit());
            }
        }
}

void PhysicsEngine::resolveCollisionWithSurface(RigidBody &b){
    //add friction
    vector3<float> norm;
   if(!Collision::hasCollided(b,norm))return;

    float sizeX = (b.geometry.end.x-b.geometry.start.x)/2 +PHYS_EPSILON,
          sizeY = (b.geometry.end.y-b.geometry.start.y)/2 +PHYS_EPSILON,
          sizeZ = (b.geometry.end.z-b.geometry.start.z)/2 +PHYS_EPSILON;

    vector3<float>& center = b.geometry.center;
    vector3<float> up = center + vector3<float>(0,sizeY,0),
                   down = center + vector3<float>(0,-sizeY,0),
                   left = center + vector3<float>(-sizeX,0,0),
                   right =  center + vector3<float>(sizeX,0,0),
                   front = center + vector3<float>(0,0,sizeZ),
                   back = center + vector3<float>(0,0,-sizeZ),
                   leftdownback = center + vector3<float>(-sizeX,-sizeY,-sizeZ),
                   rightdownback = center + vector3<float>(sizeX,-sizeY,-sizeZ),
                   leftupback = center + vector3<float>(-sizeX,sizeY,-sizeZ),
                   rightupback = center + vector3<float>(sizeX,sizeY,-sizeZ),
                   leftdownfront = center + vector3<float>(-sizeX,-sizeY,sizeZ),
                   rightdownfront = center + vector3<float>(sizeX,-sizeY,sizeZ),
                   leftupfront = center + vector3<float>(-sizeX,sizeY,sizeZ),
                   rightupfront = center + vector3<float>(sizeX,sizeY,sizeZ);

    //printf("Constraint Solver start");
    //solve constraints for primary axis first
    bool contacted=false;
    int i=0;
    do{
        i++;
        contacted=false;
        if(Collision::checkForVoxelCollision(b,up))b.translate(vector3<float>(0,-ITER_SIZE,0)),contacted=true;
        if(Collision::checkForVoxelCollision(b,down))b.translate(vector3<float>(0,ITER_SIZE,0)),contacted=true;
        if(Collision::checkForVoxelCollision(b,left))b.translate(vector3<float>(ITER_SIZE,0,0)),contacted=true;
        if(Collision::checkForVoxelCollision(b,right))b.translate(vector3<float>(-ITER_SIZE,0,0)),contacted=true;
        if(Collision::checkForVoxelCollision(b,back))b.translate(vector3<float>(0,0,ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,front))b.translate(vector3<float>(0,0,-ITER_SIZE)),contacted=true;
    }while(contacted && i<MAX_ITERATIONS);

    //solve constraits for corner cases.
    //prevents heavy reliance on corner cases to solve primary axis constraints
    contacted=false;
    i=0;
    do{
        i++;
        contacted=false;
        if(Collision::checkForVoxelCollision(b,leftdownback))b.translate(vector3<float>(ITER_SIZE,ITER_SIZE,ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightdownback))b.translate(vector3<float>(-ITER_SIZE,ITER_SIZE,ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,leftdownfront))b.translate(vector3<float>(ITER_SIZE,ITER_SIZE,-ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightdownfront))b.translate(vector3<float>(-ITER_SIZE,ITER_SIZE,-ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,leftupback))b.translate(vector3<float>(ITER_SIZE,-ITER_SIZE,ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightupback))b.translate(vector3<float>(-ITER_SIZE,-ITER_SIZE,ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,leftupfront))b.translate(vector3<float>(ITER_SIZE,-ITER_SIZE,-ITER_SIZE)),contacted=true;
        if(Collision::checkForVoxelCollision(b,rightupfront))b.translate(vector3<float>(-ITER_SIZE,-ITER_SIZE,-ITER_SIZE)),contacted=true;
    }while(contacted && i<MAX_ITERATIONS);

    b.s.velocity = refVelocity(b.d.velocity,norm);
   // printf("Constraint Solver End: i:%d\n",i);
}
